Task space angular velocity blending for real-time trajectory generation

ABSTRACT

The invention is embodied in a method of controlling a robot manipulator moving toward a target frame F 0  with a target velocity v 0  including a linear target velocity ν and an angular target velocity ω 0  to smoothly and continuously divert the robot manipulator to a subsequent frame F 1  by determining a global transition velocity v 1 , the global transition velocity including a linear transition velocity ν 1  and an angular transition velocity ω 1 , defining a blend time interval 2τ 0  within which the global velocity of the robot manipulator is to be changed from a global target velocity v 0  to the global transition velocity v 1  and dividing the blend time interval 2τ 0  into discrete time segments δt. During each one of the discrete time segments δt of the blend interval 2τ 0 , a blended global velocity v of the manipulator is computed as a blend of the global target velocity v 0  and the global transition velocity v 1 , the blended global velocity v including a blended angular velocity ω and a blended linear velocity ν, and then, the manipulator is rotated by an incremental rotation corresponding to an integration of the blended angular velocity ω over one discrete time segment δt.

ORIGIN OF THE INVENTION

The invention described herein was made in the performance of work undera NASA contract, and is subject to the provisions of Public Law 96-517(35 USC 202) in which the contractor has elected not to retain title.

BACKGROUND OF THE INVENTION

1. Technical Field

The invention relates to a compliant motion control system forcontrolling a robot using angular velocity blending in task space inperforming specific tasks.

2. Background Art

The specification below makes reference to the following publications bynumber:

References

[1] M. Brady and others (editors). Robot Motion: Planning and Control.MIT Press, Cambridge Mass., 1982.

[2] J. Canny. Collision Detection for Moving Polyhedra. IEEETransactions on Pattern Analysis and Ma

[3] J. Craig. Introduction to Robotics: Mechanics and control.Addison-Wesley, Reading, Mass., 1986.

[4] H. Goldstein. Classical Mechanics. Addison-Wesley, Reading, Mass.,1980.

[5] C. Lin and P. Chang. Formulation and Optimization of CubicPolynomial Joint Trajectories for Industrial Robots. IEEE Transactionson Automatic Control, 28(12): 1066-1073, 1983.

[6] J. Lloyd and V. Hayward. Real-time Trajectory Generation Using BlendFunctions. In IEEE International Conference on Robotics and Automation,Sacramento, Calif., April 1991.

[7] M. Mujtaba. Discussion of Trajectory Calculation Methods. StanfordUniversity, Artificial Intelligence Laboratory, AIM 285.4, 1977.

[8] R. Paul. Robot Manipulators: Mathematics, Programming and Control.MIT Press, Cambridge, Mass., 1981.

[9] R. Paul. Manipulator Cartesian Path Control, pages 245-263. MITPress, Cambridge, Mass., 1982.

[10] R. Paul and H. Zhang. Robot Motion Trajectory Specification andGeneration. In Second International Symposium on Robotics Research,Kyoto, Japan, August 1984.

[11] R. Rosenberg and D. Karnopp. Introduction to Physical SystemDynamics. McGraw-Hill, New York, 1983.

[12] H. Seraji and R. Colbaugh. Improved Configuration Control forRedundant Robots. Journal of Robotics Systems, 7(6), 1990.

[13] R. Taylor. Planning and Execution of Straight Line ManipulatorTrajectories, pages 265-286. MIT Press, Cambridge, Mass., 1982.

[14] S. Thompson and R. Patel. Formulation of Joint Trajectories forIndustrial Robots Using B-Splines. IEEE Transactions on IndustrialElectronics, 34(2): 192-199, 1987.

[15] D. Whitney. Resolved Motion Rate Control of Manipulators and HumanProtheses. IEEE Transactions on Man-Machine Systems, 10(2): 49-53, June1969.

1 Introduction

Just as manipulator control can be effectively accomplished in jointspace or task space, trajectories for the manipulator can also bespecified in joint or task space. Typically, the trajectory is specifiedin the same space in which the controller is working. However,conversion techniques can be used to translate the specified trajectoryto the control space. For instance, inverse kinematics applied to a taskspace trajectory will provide setpoints to a joint space controller.Since task space trajectory specification is usually considered mostuseful (especially with task space control), the converse translation ofa joint space trajectory to task space is uncommon.

Joint space trajectory generation is straightforward since each jointmay be treated independently [8, 1, 3]. Typically, motion betweenspecified joint values is dictated with a third, fourth, or fifth orderpolynomial. Some extension and optimization of this technique have beenproposed [5, 14].

Task space trajectory generation has been addressed more extensively,because of the complexity inherent in it. Whitney proposed Resolved Ratecontrol [15] to easily enable straight line motion or constant axisrotation of an end effector. However, this technique does not inherentlyaddress extended trajectory generation considerations. Foremost amongthese is the problem of blending changes in end effector orientation.Paul [8, 10] proposed blending of the Euler angles describing therelations of the initial and final frames to the intermediate one. Thismethod blends one orientation to the next, but the path generated is notintuitively obvious. Worse, he proposes changing one Euler angle with adifferent blend profile from the others. Alternatively, Canny [2]utilizes quaternions to describe orientation. However, since he wasaddressing a different problem (collision detection), he does notdiscuss the issues of blending the quaternions. Craig [3] utilizes thesimilar angle-axis formulation, but represents the orientation of eachvia frame with respect to the world frame, not the previous frame asPaul had done. Thus, the blend of orientation parameters will produce amotion path that is dependent on the relation of the via frames to theworld frame, not just their relation to each other. Finally, Lloyd andHayward [6] developed an elegant method for creating variable positionblend paths, but do not show an extension of the method fororientations.

As will be seen, Taylor [13] has proposed a scheme that provides smooth,intuitive, and repeatable position and orientation blends. Its majordrawback is computational complexity. This paper presents a velocitybased method that achieves the same results with a simpler formulationand significantly reduced computation time.

The next section presents the terminology employed for the solutiondescription. Section 3 presents the proposed velocity blendingformulation and described possible blend profile functions. Section 4quickly discusses position path blending. Orientation blending isextensively discussed in Section 5, where Taylor's method is reviewed,angular velocity blending is presented, and the second order differencebetween them is analyzed. Sections 6 and 7 discuss implementationalconsiderations and computational costs associated with the algorithmsand show why velocity blending is preferable. Finally, Section 8describes the results of simulation and real-time implementation.

2 Velocity Blending Terminology

A task frame is defined as the set containing the rotation matrix thatspecifies the end effector orientation, , the end effector position, p,other scalar configuration control parameters (e.g. arm angle ψ [12]),and the transit to this arm pose, T. Thus,

    F.sub.i .tbd.{.sub.i, p.sub.i, ψ.sub.i, T.sub.i }      (1)

Typically the end effector orientation is specified by a rotation matrixcomposed of the vectors defining the end effector orientation withrespect to the stationary world frame [8].

    .sub.i ═[n.sub.i, o.sub.i, a.sub.i ]                   (2)

To specify a frame, rotation matrix, or vector with respect to anotherframe, the former is proceeded with a superscript. For instance, aframe, rotation, or vector with respect to the world frame is denoted by.sup.ω F, .sup.ω , .sup.ω p.

In between two sequential frames, the desired linear velocity of the endeffector is simply the difference in position over time: ##EQU1## Theangular velocity is obtained from the equivalent angle-axis formulationfor a rotation from one frame to another [3]: ##EQU2## where motion atvelocity ω for time Δt causes a rotation of: ##EQU3## with S.sub.φ=sin.sub.φ, C.sub.φ =cos.sub.φ, and V.sub.φ =1-cos.sub.φ.

If the magnitude of Equation (5) is zero, the direction of k isindeterminant. If Equation (6) equals +1, then the orientations of thesuccessive frames are identical, and ω_(i) =[0, 0, 0]. Otherwise,Equation (6) equals -1, and k must be determined from the columns of thehomogeneous transform R=.sup.ω_(i-1) ⁻¹ .sup.ω _(i). From the firstcolumn of Equation (7) we have: ##EQU4## If k_(x) =0, another columnmust be used, and a similar set of solutions calculated.

Finally, Equations (3) and (4) may be incorporated into a globaldefinition of frame velocity:

    v.tbd.[ν,ω, ψ]                                (11)

where the scalar velocity is also calculated as in Equation (3).

It is the object of the present invention to provide a velocity blendingrobot control process which requires fewer computation steps and istherefore faster than prior art processes.

SUMMARY OF THE INVENTION

The invention is embodied in a method of controlling a robot manipulatormoving toward a target frame F₀ with a target velocity v₀ including alinear target velocity ν and an angular target velocity ω₀ to smoothlydivert the robot manipulator to a subsequent frame F₁, the target framebeing associated with a target transition time T₀ and the subsequentframe being associated with a subsequent transition time T₁, bydetermining a global transition velocity v₁ necessary to move themanipulator from the target frame F₀ to the subsequent frame F₁ withinthe subsequent transition time T₁, the global transition velocityincluding a linear transition velocity ν₁ and an angular transitionvelocity ω₁, defining a blend time interval 2τ₀ within which the globalvelocity of the robot manipulator is to be changed from the globaltarget velocity v₀ to the global transition velocity v₁ and dividing theblend time interval 2τ₀ into discrete time segments δt. During each oneof the discrete time segments δt of the blend interval 2τ₀, thefollowing is performed: (a) compute a blended global velocity v of themanipulator as a blend of the global target velocity v₀ and globalsubsequent velocity v₁, the blended global velocity v being at leastapproximately equal to the target global velocity v₀ at the beginning ofthe blend time interval and at least approximately equal to the globaltransition velocity v₁ at the end of the blend time interval, theblended global velocity including a blended angular velocity ω and ablended linear velocity ν, and then, (b) rotate the manipulator by anincremental rotation corresponding to an integration of the blendedangular velocity ω over one discrete time segment δt.

BRIEF DESCRIPTION OF THE DRAWINGS

FIGS. 1A and 1B are graphs showing the blend speed for a spectrum ofangles (0, 45, 90, 135 and 180 degrees) between the initial and finalvelocities, for the case in which the magnitudes of the initial andfinal velocities are equal using linear velocity blending (FIG. 1A) andthird order polynomial velocity blending (FIG. 1B).

FIGS. 2A and 2B are graphs comparing linear, third order polynomial andcycloidal velocity blends of two orthogonal velocities of equalmagnitude (FIG. 2A) and two parallel velocities of unequal magnitude(FIG. 2B).

FIGS. 3A and 3B are graphs showing the spatial paths (FIG. 3A) andtemporal paths (FIG. 3B) for a transition between two orthogonalvelocities of equal magnitudes for a maximum acceleration magnitude of10m/s².

FIG. 4 is a graphical depiction of the velocity blending process ofEquation 36.

FIGS. 5A and 5B are diagrams depicting the spatial transition of thetarget frame (FIG. 5A) and the angular velocity vector (FIG. 5B) duringan orientation blend using the process of Equation 36 with linearblending.

FIGS. 6A, 6B and 6C are graphs illustrating the component values of theunit vectors of the frames shown in FIG. 5A for the n, o and αcomponents, respectively.

FIG. 7 is a graph depicting the incremental blending of the process ofEquation 47.

FIG. 8 is a graph depicting the blending of the process of Equation 57in accordance with the present invention.

FIGS. 9A and 9B are diagrams depicting the spatial transition of thetarget frame (FIG. 9A) and the angular velocity vector (FIG. 9B) duringan orientation blend using the process of Equation 57 with linearblending.

FIGS. 10A, 10B and 10C are graphs illustrating the component values ofthe unit vectors of the frames shown in FIG. 9A for the n, o and acomponents, respectively.

FIG. 11 is a simplified schematic block diagram of a robot controlsystem employed in carrying out the invention.

FIG. 12 is a block flow diagram illustrating the blending process of thepresent invention in accordance with a preferred embodiment.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

3 Angular Velocity Blending

To move smoothly from one segment to another, the velocities of thesegments must be blended together. To achieve this, many strategiesemploying linear velocity ν have been suggested [9, 13, 5, 14, 6, 7].These techniques are discussed below within the framework of the presentinvention.

The present invention introduces the concept of blending angularvelocity by blending a global velocity vector v that includes both anangular velocity vector ω and the linear velocity vector ν. Thefollowing discussion utilizes the global velocity v of Equation 11 whichincludes angular velocity ω with the following convention: ##EQU5##where 2τ is the blend period, dependent on the maximum allowedacceleration, as will be shown below. This implies that the normalizedtime parameter s ε [0, 1].

To smoothly blend from v_(a) to v_(b) over the interval s, the presentinvention employs a normalized blending function ƒ'(s) ε [0, 1].Utilizing this function, the velocity profile during the blend is:

    v=v.sub.a (1-ƒ'(s))+v.sub.b ƒ'(s)        (16)

    =v.sub.a +(v.sub.b -v.sub.a)ƒ'(s)                 (17)

and the acceleration is: ##EQU6## Note that this formulation ensureszero acceleration for v_(a) =v_(b). Also, there is spatial symmetry ofthe path for the case of |v_(a) |=|v_(b) |, because the accelerationvector is parallel to the difference of the two velocity vectors, andwill therefore bisect them.

If the maximum allowed acceleration is specified, then the blend periodmay be determined: ##EQU7## assuming that the derivative of ƒ'(s) is asymmetric function with a maximum value at s=0.5.

There are several simple choices available for blend functions. Theseare provided below, along with the resultant form of the velocity,acceleration, and blend time.

Linear Velocity Blending [13] ##EQU8## Third Order Polynomial VelocityBlending [9, 5] ##EQU9## Cycloidal Velocity Blending [7] ##EQU10## Thecycloid has a functional form very close to that of the O(3) polynomial,but does not have a discontinuous jerk (the derivative of theacceleration). In turn, the O(3) polynomial is superior to the linearform since the latter has discontinuous acceleration (and infinitejerk). The strength of the linear form is that it requires the leasttime since the acceleration is applied constantly at the maximum valueallowed. Finally, note that many other functions are possible; inparticular, all odd order polynomials.

The graphs of FIG. 1 show the blend speed versus time for a spectrum ofangles (0,45,90,135,180 deg) between the initial and final velocityvectors for the case of |v_(a) |=|v_(b) |, |a|_(max) =10 m/s². FIG. 1Ashows the speeds for linear velocity blending. FIG. 1B shows the speedsfor third order polynomial blending. The profiles for cycloidal blendingare extremely close to those shown in FIG. 1B. The cusp in the plot for180 degrees is due to a change in direction, and does not indicate adiscontinuity in the acceleration. Also note that when the initial andfinal velocities are equal the speed is constant across the blend.

The graphs of FIG. 2 shows a comparison of linear, third orderpolynomial, and cycloidal velocity blends, with |a|_(max) =10 m/s². FIG.2A shows the blend speed for a transition between two velocities ofequal magnitude at an angle of 135 degrees. FIG. 2B shows a transitionbetween two velocities of unequal magnitude. In this figure, the initialvelocity is zero, however the transition curve has the same shape fortwo non-zero parallel velocities. Further, Equation (17) shows that thisform of the blending occurs for each component of the resultant velocityvector.

4 Blending the Position Trajectory

The blend of the end effector position (p) is described by directintegration of Equation (3). (Scalar quantities are handled in the sameway.) This yields:

    p=∫ν(s)dt=2τ∫ν(s)ds                    (30)

    =p.sub.o +v.sub.a 2τs+(ν.sub.b -ν.sub.a)2τ ∫ ƒ'(s)ds                                          (31)

    =p.sub.o +v.sub.a 2τs+(ν.sub.b -ν.sub.a)2τ ƒ(s) (32)

where p_(o) is the initial position as the blend is entered. The form ofthe integral of the blend function determines the spatial form trace bythe path. For the three blend functions considered, we have: ##EQU11##

Equation (33) provides a second order polynomial, and the blend isparabolic. Equation (34) provides a fourth order polynomial, and theblend that is steeper. (Higher order even polynomial functions will beincreasingly steeper.) The cycloidal blend path remains sinusoidal, buthas the addition of a linear term.

The graphs of FIG. 3 show the spatial and temporal paths for atransition between v_(a) and v_(b), such that v_(a) ⊥v_(b), |v_(a)|=|v_(b) |, with |a|_(max) =10 m/s². It is apparent from FIG. 3A thattighter cornering can be accomplished with polynomial and cycloidalbleeding. However, this requires longer blend times (or largeracceleration, and therefore greater joint torques from the actuators).FIG. 3B shows the positions as a function of time, which are essentiallythe integrals of the velocities shown in FIG. 3B. The form of thesecurves also represents the functional form of the position blendfunctions, Equations (33)-(35).

5 Blending the Orientation

Blending of the orientation is more complicated than position, since theangular velocities are nonholonomic. However, this section shows that aclose approximation to analytic orientation blending can be obtained.This requires numeric integration of the rotations obtained from theinstantaneous value of the blended angular velocity.

5.1 Rotation Matrix Blending for Orientation

In reference [13] Taylor proposed a method of blending orientation basedon rotation matrices. A generalization of this method will be presentedhere. In this method, the amount of rotation contributed by eachrotation matrix is scaled with the previously presented blend functions:

    .sup.ω (s)=.sup.ω .sub.o .sup.o .sub.a [ω.sub.a 2τ(s-ƒ(s))] .sup.a .sub.b [ω.sub.b 2τƒ(s)](36)

    =.sup.ω .sub.o .sup.o .sub.α .sup.α .sub.β(37)

The graph of FIG. 4 provides a graphical depiction of this blendingmethod. Prior to the blend there is motion away from the orientation ofthe previous frame, F_(i-1), and toward the intermediate orientation,a=F_(i). The constant angular velocity before the blend is ω_(a), andthe blend begins at orientation o. In this method, for each intervalafter o a rotation is construction and applied according to the rotationmatrix blending described by Equation (36) or (37). After the normalizedblend time s has become unity, the commanded angular velocity will beω_(b), and the commanded orientation is b. After this time, thetrajectory continues toward the next target frame, F_(i+1), at theconstant angular velocity of ω_(b). To avoid faceted motion through theblend, the normalized time must be incremented in infinitesimalintervals.

In reference [13], the formulation of this blending scheme is presentedwith respect to frame a, not o. This alternate representation can beseen by starting with Equation (36), and utilizing the identity.

    .sup.ω .sub.o .sup.o .sub.α =.sup.ω .sub.a .sup.a .sub.o .sup.o .sub.α                                       (38)

    =.sup.ω .sub.a .sup.a .sub.o [ω.sub.a τ].sup.o .sub.α(39)

    =.sup.ω .sub.a .sup.o .sub.a [-ω.sub.a τ].sup.o .sub.α(40)

we have:

    .sup.ω (s)=.sup.ω .sub.o .sup.o .sub.a [ω.sub.a 2τ(s-ƒ(s))].sup.a .sub.b [ω.sub.b 2τƒ(s)](41)

    =.sup.ω .sub.a .sup.o .sub.a [-ω.sub.a τ] .sup.o .sub.a [ω.sub.a 2τ(s-ƒ(s))] .sup.a .sub.b [ω.sub.b 2τƒ(s)]                                      (42)

    =.sup.ω .sub.a .sup.o .sub.a [ω.sub.a 2τ(s-ƒ(s)-1/2)] .sup.a .sub.b [ω.sub.b 2τƒ(s)]                                      (43)

Further, reference [13] only considers the linear blend case withƒ(s)=1/2s². This gives:

    .sup.ω (s)=.sup.ω .sub.a .sup.o .sub.a [-ω.sub.a τ(1-s).sup.2 ] .sup.a .sub.b [ω.sub.b τs.sup.2 ](44)

Substituting Equations (4), (14), and (15) yields: ##EQU12##

This is the form of the rotation blend presented in [13].

The diagrams of FIG. 5 provide a graphical depiction of change in thetarget frame (FIG. 5A) and the direction of the angular velocity vector(FIG. 5B). (A constant spatial velocity has also be used, to spread outthe vectors for pictorial clarity.) The graphs of FIG. 6 show the changein the target frame basis vector components under this transformation.

5.2 Incremental Rotation Blend Components

It is informative to look at the rotations that represent the individualincremental rotation between successive time increments when utilizingEquation (36). Consider the difference between successive framesdepicted in FIG. 7.

The incremental rotation between successive orientations is:

    .sup.ω .sub.β' =.sup.ω .sub.β .sup.β .sub.β'(46)

    .sup.β .sub.β' =.sup.ω .sup.-1.sub.β .sup.ω .sub.β'                                              (47)

    =.sup.α .sup.-1.sub.β .sup.o .sup.-1.sub.α .sup.ω .sup.-1.sub.o .sup.ω .sub.o .sup.o .sub.α' .sup.α' .sub.β'                                              (48)

    =.sup.α .sup.-1.sub.β .sup.o .sup.-1.sub.α .sup.o .sub.α' .sup.α' .sub.β'                  (49)

    ≈.sup.α .sup.-1.sub.β (1+.sup.αε.sub.α') .sup.α' .sub.β'(50)

    ≈.sup.α .sup.-1.sub.β .sup.α' .sub.β' +.sup.α .sup.-1.sub.β .sup.α ε.sub.α' .sup.α' .sub.β'                                (51)

    ≈(1+.sup.β ε.sub.β')+.sup.α .sup.-1.sub.β .sup.α ε.sub.α' .sup.α' .sub.β'                                              (52)

    ≈1+.sup.β ε.sub.β' +.sup.β ε.sub.α'                                    (53)

    ≈.sup.β [ε.sub.β' +ε.sub.α' ](54)

    ≈.sup.β [ω.sub.b (s.sub.β)Δs+ω.sub.a (s.sub.β)Δs]                                   (55)

    ≈.sup.β [ω(s.sub.β)Δs]       (56)

where ε is the infinitesimal rotation operator [4]. This resultindicates each incremental rotation of Taylor's scheme is equal, tofirst order, to the rotation provided by the instantaneous angularvelocity. This implies that it is possible to blend the angularvelocities utilizing Equation (17), and obtain the incremental rotationsfrom the value of the instantaneous angular velocity.

5.3 Angular Velocity Blending for Orientation

As was discussed in the last section, the incremental rotations of anorientation blend may be approximated by utilizing the instantaneousangular velocity provided by Equation (17). Thus, the orientation of thetarget frame can be computed by utilizing Equations (1), (4), (7), (11),and (17): ##EQU13## where N is the total number of steps for thecomplete blend. FIG. 8 provides a graphical depiction of this blendingmethod. Before the blend, there is motion away from the orientation ofthe previous frame, F_(i-1), and toward the intermediate orientation,α=F_(i). The constant angular velocity before the blend is ω_(a). Theblend begins at orientation o. For each interval after o, a rotation isconstructed and applied according to the angular velocity blendingprovided by Equation (17). After the normalized blend time s has becomeunity, the commanded angular velocity will be ω_(b). Ideally, the blendwill be complete at the desired orientation, o, where the trajectorycontinues toward the next target frame, F_(i+1).

In practice, velocity-based blending can provide equivalent blends tothe rotation matrix method described previously. The graphs of FIG. 9depict the change in the target frame (FIG. 9A) and the direction of theangular velocity vector (FIG. 9B) for third order polynomial angularvelocity blending, with |a|_(max) =10 m/s². A constant linear velocityis also utilized to spread out the origins of the frames for clarity.The graphs of FIG. 10 show the change in the target frame basis vectorcomponents under this transformation. Comparing FIGS. 9 and 10 withFIGS. 5 and 6, it is seen that there is little difference betweenblending schemes, even when using different blending profiles.

5.4 Compensation for Second Order Error from Angular Velocity Blending

Looking closely at FIG. 10, it can be seen that there is some smallresidual error in the components of the basis vectors. This errorresults from the second order error introduced by the infinitesimalrotation approximation in Section 5.2. This can be understood byconsidering how the angular velocity blending effects the rotationblending. Consider first the case of total completion of rotation byω_(a), before rotation by ω_(b) begins. In this case, the resultingrotation is exact:

    .sup.o.sub.b =.sup.o .sub.a [ω.sub.a τ].sup.a .sub.b [ω.sub.b τ]                                     (58)

    =.sup.o .sup.1.sub.a .sup.o .sup.2.sub.a . . . .sup.o .sup.N-1.sub.a .sup.o .sup.N.sub.a .sup.a .sup.1.sub.b .sup.a .sup.2.sub.b . . . .sup.a .sup.N-1.sub.b .sup.a .sup.N.sub.b                        (59)

where the rotations ^(o) _(a) and ^(a) _(b) have been divided into Nparts. Blending the angular velocities is equivalent to changing theorder of some of the rotations at the center of this chain. Forinstance, utilizing the infinitesimal rotation approximation [4]:

    .sup.o.sub.b ≈.sup.o .sup.1.sub.a . . . .sup.o .sup.N-1.sub.a (1+.sup.o ε.sub.a.sup.N)(1+.sup.a ε.sub.b.sup.1) .sup.a .sup.2.sub.b . . . .sup.a .sup.N.sub.b                    (60)

    ≈.sup.o .sup.1.sub.a . . . .sup.o .sup.N-1.sub.a (1+.sup.a ε.sub.b.sup.1)(1+.sup.o ε.sub.a.sup.N) .sup.a .sup.2.sub.b . . . .sup.a .sup.N.sub.b                                 (61)

This commutation of the infinitesimal rotations may be continued untilthe proper sequence is attained. However, second order errors arise fromthe initial approximation of ≈(1+ε) and from the disregard of thecommutator (the difference between the sequence of the rotations):

    [1+ε.sub.A, 1+ε.sub.B ]=2ε.sub.A ε.sub.B -2ε.sub.B ε.sub.A                         (62)

The lack of these second order terms explains the small error introducedby angular velocity based orientation blending.

The change in position of ^(o) _(a) ^(i) and ^(a) _(b) ^(j) operators inthe sequence is reminiscent of diffusion. As the ^(o) _(a) ^(i)`diffuse` farther to the right, and the ^(a) _(b) ^(j) `diffuse` fartherto the left, the changed in orientation becomes more blended. Since theinfinitesimal rotations can be represented by their angular velocityequivalents, the diffusion profile is equivalent to the velocity blendprofile. For instance, the shape of the cycloidal blend profile in FIG.2B indicates more diffusion than the linear one. Further smaller valuesof |a|_(max) also imply more diffusion, since they spread out thesecurves. More diffusion introduces second order error. Therefore, linearblends and high acceleration blends result in less residual error for agiven value of |a|_(max). However, linear blends will result in moreerror if the blend time is fixed instead of the acceleration. This canbe understood by lessening the slope of the linear blend line in FIG.2B, thus introducing more diffusion.

To provide some quantitative description to this discussion, thefollowing table shows the magnitude of the orientation error for theexample previously considered.

    ______________________________________                                        blend type  |a|.sub.max = 10 m/s.sup.2                                                |a|.sub.max = 5                   ______________________________________                                                                  m/s.sup.2                                           linear      0.29°  1.16°                                        O(3) polynomial                                                                           0.39°  1.56°                                        cycloidal   0.41°  1.62°                                        ______________________________________                                    

It is apparent that these errors are small and may be corrected (asdescribed below). Substantially larger errors are not possible sincethey would require much smaller accelerations which require longer blendtimes. Too large of a blend time multiplied by ω_(a) or ω_(b) wouldindicate a rotation greater than 180° in the initial or final legs. Suchlarge rotations have been precluded by Equation (5).

While this small error introduced by one blend does not necessarilyrequire compensation, the summation of this error over successive blendsmay become significant. To compensate for the residual error, we proposethe use of a correction term which is calculated at the end of everyvelocity based blend of orientation. This term is the incrementalrotation from the resultant frame to the desired frame at the end of theorientation blend:

    [k.sub.cor,  .sub.cor ]=(.sup.ω .sub.a .sup.a .sub.b [ω.sub.a τ]).sup.-1 ω [s.sub.N =1]                       (63)

In practice, k_(cor) and _(cor) can be easily calculated by Equations(5) and (6). A correction velocity may then be calculated and applied tothe leg of the trajectory being entered, for the time specified to thenext via frame:

    ω.sub.cor =k.sub.cor cor /(T.sub.i+1 -τ.sub.i)   (64)

This correction term is modified by a gain, K_(cor) and added to theangular velocity ω_(b). (Since it is very small in magnitude, concernsabout changing the value of ω_(b) have been ignored.) The gain is neededto maintain stability in what is effectively a low bandwidth feedbackcontroller. If Equations (57) and (63) were linear, this discreet timecontroller would be trajectory stable for 0≦K_(cor) ≦1. However, for thenonlinear orientation blending, we have empirically found stability forgains of 0≦K_(cor) ≦0.3.

6 Implementation Considerations

Three main implementational considerations have been accommodated in ourscheme: maximum acceleration, minimum blend time, and velocitysummation.

6.1 Maximum Acceleration

Since the calculated trajectories are to be executed by realmanipulators, the commanded acceleration must be limited to what isachievable. Further, the achievable task space acceleration of the armdepends on the configuration of the robot arm. In different parts of theworkspace, different task space accelerations are possible. Therefore,two possibilities exist: 1.) limit all task space accelerations to theworst case acceleration of the arm, or 2.) create a complete map of theachievable task space accelerations, and limit the trajectory blendingaccordingly. However, creating and accessing such a map is anticipatedto be very cumbersome. Therefore, we have currently chosen to work withthe first, and simpler, of these two options.

Another consequence of limited acceleration is that it erodes thestraight line leg segments of the trajectory between via frames. For asmall enough acceleration, one blend will end as another begins. Foraccelerations smaller than this, one blend would have to begin beforeanother ends. We do not permit this to occur. In this case, theacceleration is increased level of acceleration is not achievable by thearm, then the via frames are not reasonably selected and unavoidableposition errors will occur.

6.2 Minimum Blend Time

Due to the discrete nature of the computer implementation of thesealgorithms, it is necessary to specify a minimum number of iterationsover which an acceleration is specified. From Equation (b 20) thisquantity is the minimum allowed value of 2τ. If a minimum is notspecified, the calculated blend time may become comparable to thealgorithm cycle time. Thus, the calculated velocity and position will bediscontinuous, providing poor input to the arm controller. We haveempirically determined and utilized a minimum value of twenty iterationsper blend. A direct consequence of this specification of 2τ_(min) isthat the maximum allowed acceleration is also limited. If moreacceleration is desired, and the manipulator is capable of it, then2τ_(min) should be reduced. However, to keep the same number ofiterations per blend with a reduced 2τ_(min), the algorithm rate must beincreased proportionally.

6.3 Velocity Summation

To be able to modify commanded trajectories with other control inputs,the commanded variable must be a velocity (a generalized flow variable),not a position [11]. FIG. 11 shows a system for implementing oneembodiment of the invention. Although not shown in the drawing, thetrajectory generator optionally may be subject to modification by theinput of a joystick or a proximity sensor monitor process. In FIG. 11, atrajectory generator 10 performs the velocity blending process describedabove to produce a desired sequence of desired end effector frames orpositions. These are output to a manipulator control system Cartesiancontroller 20. The controller 20 computes and outputs a command angleθ.sub.τ to a robot arm controller 30 (in this case, the arm controllerfor the Robotics Research K-1207 Arm). The arm controller 30 convertsthe command angle to motor currents and outputs the motor currents toservoes in a robot arm 40 (in this case, a Robotics Research K-1207Arm). The robot arm 40 returns servo encoder counts to the armcontroller 30, which computes therefrom and outputs corresponding anglemeasurements θ_(m) to a forward kinematics processor 50. The forwardkinematics processor 50 computes and outputs a corresponding measuredframe F_(meas) =x_(m) to the trajectory generator 10 and computes andoutputs a Jacobian transformation matrix J to the manipulator controller20. The Cartesian controller 20 and the forward kinematics processor 50perform the foregoing operations and computations using conventionaltechniques well known in the art. The arm controller 30 and the roboticsarm 40 are commercially available devices.

Utilizing the velocity blending scheme described in this specificationwith reference to Equation 57, velocity output is obtained directly.Alternatively, if analytic integration of position is used (as inEquation (32)), or if rotation matrix orientation blending is used (asin Equation (36)), then the velocity must be obtained by differencingthe reference frames. As will be seen in the next section, this requiresextra computation not needed with a purely velocity based scheme.

7 Computational Costs

Table I provides an outline of the computational steps and costs forboth position-based and velocity-based blending. The equations involvedin each step are also summarized. Finally, an estimate of thecomputational complexity is given by stating the number of additions,subtractions, multiplies, and divides required, as well as thetrigonometric (and square root) operations needed.

Under the operations column, the values are the number of standard mathoperations (+-*/) and the number of trigonometric and other mathoperations (e.g., sin, cos, sqrt, and so forth). The top section of thetable reviews some common steps needed for both schemes. Of these, theframe differencing and frame incrementing are very costly. Thecalculation of ƒ(s) or ƒ'(s) is variable since it depends on the blendfunctions chosen.

The second and third sections of the table show the algorithmicdifferences between the position/orientation blending and the velocityblending methods. The most striking difference between the twoformulations is the reduced computational cost of the velocity blendingmethod. During a blend it requires only 12 operations, while theposition/orientation method requires 263 operations plus eight costlytrig or square root calls. The situation is much the same during thestraight line leg segments of the trajectory, where the velocity basedscheme requires zero operations, while a completely position basedscheme requires 160 plus 5. The efficiency of the velocity based schemeis paid for by the overhead necessary during the transition from blendto leg segments. At this juncture, the velocity scheme must make 207plus 6 operations, while the position/orientation scheme requires only69 plus 2. However, this overhead occurs only once for each via frame,compared to the hundred or thousands of iterations that occur for theblend or leg segment

                  TABLE I                                                         ______________________________________                                        Algorithm Step      Eqns       Ops                                            ______________________________________                                        Common                                                                        vΔt = framedif(F.sub.1,F.sub.2) = D()                                                       1-6, 11    69, 2                                          F.sub.2 = frameinc(F.sub.1,vΔt) = I()                                                       1-4, 7-11  91, 3                                          calc f(s) or f'(s)  17, 32, 36 variable                                       v.sub.2 = vecscale(v.sub.1,func) = S()                                                            11         7, 0                                           a < |a|.sub.max, τ > τ.sub.min                                          20         variable                                       Position/Orientation Blending Method                                          blend                                                                         calc f(s)           33, 35, 34 variable                                       v.sub.α  = S(v.sub.a,s - f(s))                                                              36         6                                              v.sub.β  = S(v.sub.b,f(s))                                                                   36         6                                              F.sub.a = I(F.sub.o,v.sub.α Δt)                                                       36         91, 3                                          F.sub.b = I(F.sub.a,v.sub.β Δt                                                         36         91, 3                                          v = D(F.sub.o,F.sub.b)/Δt.sub.ob                                                            1-6, 11    69, 2                                          leg                                                                           F(t) = I(F.sub.i,{p(t),kφ(t),ψ(t)})                                                       1-4, 7-11  91, 3                                          v = D(F(t),F(t - Δt))/Δt                                                              1-6, 11    69, 2                                          transition                                                                    v.sub.b = D(F.sub.i,F.sub.i+1) /T.sub.i+1                                                         1-6, 11    69, 2                                          a < |a|.sub.max, τ > τ.sub.min                                          20         variable                                       Velocity Blending Method                                                      blend                                                                         calc f'(s)          21, 27, 24 variable                                       v = S(v.sub.a,1 - f'(s))                                                                          17         6                                              v + = S(v.sub.b,f'(s))                                                                            17         6                                              leg                                                                           nothing, constant v = v.sub.a  0, 0                                           transition                                                                    v.sub.b = D(F.sub.i,F.sub.i+1) /T.sub.i+1                                                         1-6, 11    69, 2                                          F'.sub.b = I(F.sub.1,v.sub.b τ.sub.i)                                                         63, 64     69, 2                                          v.sub.b + = D(F.sub.b,F'.sub.b)/T.sub.i+1 - τ.sub.i)                                          63, 64     69, 2                                          a < |a|.sub.max, τ > τ.sub.min                                          20         variable                                       ______________________________________                                    

computations. Obviously, velocity blending introduces a significantcomputational savings.

It is important to note that some of the computational advantage ofvelocity blending is introduced by the assumption that the output of atrajectory generator must be a velocity. The position/orientation schememust utilize a velocity calculation step during the blend and legsegments which costs 69 plus 2 operations. However, even without thisstep the velocity blending method is significantly faster. Further, itwas shown in the last section why velocity output is more useful.

One other computational burden is introduced to the position/orientationmethod by the assumption that position, [p,kφ,ψ], is specified as afunction of time during the leg segment. Alternatively, the leg segmentvelocity could be precomputed and utilized directly as in the velocityblend method. Since k is constant during the leg segment, no errorswould be introduced. Also, the leg velocity must be computed anyway ifthe maximum acceleration checks are to be performed (as is assumed).

8 Implementation

We have implemented this algorithm on an SGI Iris workstation forsimulation, and on a VME based 68020 microprocessor for control of 7 DOFRobotics Research K-1207 Arm. The end-effector of the robot arm carriesan array of sensors: two CCD cameras, two proximity sensors, an opticalpyrometer, a gas sensor, and a force sensor. The addition ofeddy-current and contact acoustic sensors are planned. While our frameto frame motions are designed to aid inspection by these devices, thepresented technique is obviously extensible to motion required forpurposes other than inspection.

8.1 Experimentation

The blending algorithm has also been implemented for real-time controlon a 12.5 MHz Heurikon 68020 processor. For the tests, a trajectorysimilar to the simulation trajectory has been executed. However, sincethe robot base position is fixed, the size of the inspection area isrestricted. A total of twelve via frames are used to scan a rectangularshape about half as large as that in the simulation. Linear blending wasarbitrarily chosen for these tests. During experiments the minimum timebetween frames is 3 seconds. The real-time process runs at 44 Hz, or≈22.7 ms, giving approximately 132 iterations for each frame to framemotion. (The control rate is governed by other control software, not theprocessing requirements of the trajectory blending algorithm, which wehave shown to be quite minimal.) The position gain was K_(p) =20, andthe trajectory correction gain was K_(cor) =0.3. The minimum blend timewas 2τ_(min) =20 iterations, or about a half second. The maximumacceleration was |a|_(max) =10 m/s².

9 Angular Velocity Blending Processing Description

The angular velocity blending method described above is now described ingreater detail with reference to the steps depicted in FIG. 12.

The process begins by initializing key parameters (block 100 of FIG.12), by setting the index n to one, setting the current global velocityv an the initial velocity v₀ to zero, while setting the current frame Fto the measured frame of reference F_(meas) and setting the current timet to the minimum blend time τ_(min). The previous frame corresponds toF_(i-1) of the graph of FIG. 8.

Typically, the user specifies the next target frame F₀ corresponding toF_(i) or point a of FIG. 8, the subsequent frame F₁ corresponding toF_(i+1) of FIG. 8. As a slight departure from the notation employed inFIG. 8, the process illustrated in FIG. 12 employs the index n to keeptrack of the successive frames, and the next target frame F₀ is set toF_(n-1) while the subsequent frame F₁ is set to F_(n).

The description of this process will now skip to a point at whichblending has been completed for a current frame, so that the index n isto be incremented by one: n=n+1. This incrementing of the index n isperformed as part of an increment step of block 85, which begins a newiteration of the cyclic process. In the increment step of block 85, thecurrent time is shifted by T₀, the current target frame F₀ is updated tothe subsequent frame F₁ of the previous iteration and the current targetframe F₁ is set to the next frame F_(n) specified by the user. Theglobal velocity v includes both the linear velocity ν and the angularvelocity ω, in accordance with Equation 11. In the increment step ofblock 85, the initial block velocity v₀ is corrected by an errorcorrection global velocity v_(e) computed in an other part of theprocess in accordance with Equation 65 in a manner described belowherein. As will be described, the purpose of this correction is tocompensate for a residual error at point b of FIG. 8 corresponding to ablend exit frame F'₀, specifically the residual error discussed withreference to Equation 63.

Next, a differentiation step of block 90 is performed using acomputation described below called framedif to compute a global velocityv₁ necessary to move from frame F₀ to frame F₁ within a time T₁specified by the user. The step of block 90 then computes from the twoglobal velocities v₁ and v₀, and from a maximum acceleration specifiedby the user, a blend interval time τ₀ in accordance with Equation 23, 26or 29 using a process calctau described later in this specification.

At a decision block 120, a determination is made whether the blend timet=T₀ -τ₀ has been reached. If not (YES branch of block 120), then thecurrent global velocity v is kept constant at v₀ (block 125) and thetime t is incremented by adding to it a time differential δt (block130).

A frame incrementing step 135 is performed using the current angularvelocity ω of the current global velocity v. This frame incrementingstep 135 is a rotation of the manipulator though an incremental angleequal to ω×δt. The frame incrementing step of block 135 updates thecurrent frame F. The process then cycles back to the determination stepof block 120 and continues in a cycle constituting the steps of blocks120, 125, 130 and 135. This cycle is repeated until the time t reachesthe blend time t=T₀ -τ₀ (NO branch of block 120).

Once the blend time has been reached, a determination is made at adecision block 140 whether the current time t falls within the blendtime window T₀ -τ₀ <t<T₀ +τ₀. If so (YES branch of block 140), then theblend function ƒ'(t,s) is computed (block 145) according to Equation 33,34 or 35 and this function is used to update the current blended globalvelocity v using Equation 16. The current time is incremented in thestep of block 150 as in the step of block 130. The frame incrementingstep of block 135 is performed, but this time using the current angularvelocity ω of the global velocity v blended between v₀ and v₁ inaccordance with Equation 16 by the step of block 145. The process cyclesback to the determination step of block 140. A cycle constituting blocks140, 145, 150 and 135 is repeated until the time t exceeds the blendperiod (NO branch of block 140). Each iteration of this cycle producesan incremental rotation of the frame using an angular velocity vector ωupdated each iteration.

Upon completion of this cycle (i.e., when t>T₀ +τ₀), a sequence ofincremental rotations has been performed as depicted in FIG. 8 to blendthe manipulator motion from the initial frame F₀ to the blend exit frameF'₀. With each iteration of the process after each time increment δt,the frame increment step of block 135 computes an updated frame F, whichis output by the trajectory generator 10 of FIG. 11 to the manipulatorcontrol system cartesian controller 20 of FIG. 11 to produce a commandθ_(r) to the robot servos to rotate and/or translate the robotmanipulator to the updated frame.

Taking the NO branch of block 140, the current frame F is compared tothe desired blend exit frame F'₀ (corresponding to point b of FIG. 8)obtained from a frame incrementing step 105. The frame incrementing stepis an incremental rotation through an angle obtained by multiplying theinitial angular velocity ω₀ by half the blend time, τ₀, obtained fromthe step of block 90. (Both frame incrementing steps 105 and 135 employa process called frameinc which is described later in thisspecification.)

Then, a differentiation step 110 computes a velocity error correctionv_(e) by dividing the difference between the current frame F and thedesired blend exit frame F'₀ by the time remaining to the next frame, T₀-τ₀. The incrementing step of block 85 is repeated, and the entireprocess begins the next iteration with a new target frame F_(n+2). Theadding step v₀ =v₀ v_(e) of block 85 compensates for the residual errorof the previous blend cycle and implements the correction of Equation65. (The differentiation steps of both blocks 90 and 110 employ aprocess called framedif defined later in this specification.)

The foregoing process is now set forth in greater detail as a series ofprogram language statements, each statement being accompanied by anexplanatory remark in italics. In the following, there is a mainprogram, called main body which calls for four different sub-routinesnamed, respectively, frameinc, framedif, calctau and calcƒprime.

9.1 Main Body

    ______________________________________                                        BEGIN:                                                                        n = 0  initialize counter                                                     F = F.sub.meas  initialize desired to current frame                           F.sub.0 = {.sub.0,p.sub.0,ψ.sub.0,T.sub.0 }  next target frame            v = 0  start at rest                                                          v.sub.0 = 0  initial velocity is zero                                         v.sub.e = 0  initial error correction velocity is zero                        t = τ.sub.min  initialize time                                            NEXT:                                                                         v.sub.0 = v.sub.0 + v.sub.e  modify target velocity                           n = n + 1  increment counter                                                  F.sub.1 = F.sub.n = {.sub.n,p.sub.n,ψ.sub.n,T.sub.n }  subsequent         frame (if unavailable                                                         set to F.sub.n-1)                                                             V.sub.1 = framedif (F.sub.0,F.sub.1)/T.sub.1  determine average velocity      needed between frames                                                         τ.sub.0 = calctau(v.sub.1 - v.sub.0)  calculate and shift blend time      while(t < T.sub.0 + τ.sub.0){                                             t = t + δt  increment time                                              LEG:                                                                          if(t < T.sub.0 - τ.sub.0) v = v.sub.0  constant velocity in leg           BLEND:                                                                        if(T.sub.0 - τ.sub.0 < t < T.sub.0 + τ.sub.0){                        f' = calcfprime((t - T.sub.0 + τ.sub.0)/2τ.sub.0)  get blend          function                                                                      value                                                                         v = v.sub.0 (1 - f') + v.sub.1 f'  calculate blended velocity                 F = frameinc(F,vδt)  increment the desired frame                        }                                                                             if(t ≧ T.sub.0 + τ.sub.0){                                         F'.sub.0 = frameinc(F.sub.0,v.sub.0 τ.sub.0)  determine correct frame     position                                                                      for blend exit                                                                v.sub.e = framedif(F,F'.sub.0)/(T.sub.0 - τ.sub.0)  determine             additional velocity                                                           needed                                                                        t = t - T.sub.0  shift time                                                   F.sub.0 = F.sub.1  shift frames                                               v.sub.0 = v.sub.1  shift velocity value                                       goto NEXT:  repeat the process                                                }                                                                             ______________________________________                                    

9.2 Frame Differencing Subroutine

    ______________________________________                                        framedif(F.sub.0,F.sub.1)  frame differencing subroutine                      v = (p.sub.1 - p.sub.0)  linear velocity assuming unit time                    ##STR1##                                                                      ##STR2##                                                                     if(|k sinφ| = 0) { ambiguous result                     if(cosφ = 1) φ = 0  no difference in frames                           if(cosφ = -1) { greatest difference in frames                             φ = π                                                                   ##STR3##                                                                     k.sub.y = n.sub.y /2k.sub.x  if k.sub.x (o.sub.x) = 0, substitute             a.sub.x, a.sub.y,a.sub.z                                                      k.sub.z = n.sub.z /2k.sub.x                                                   }                                                                             }                                                                             if(|k sinφ| ≠ 0) φ = tan.sup.-1 (sinφ,     cosφ) minimum angle                                                      between frames                                                                 ##STR4##                                                                     ψ = (ψ.sub.1 - ψ.sub.0)  scalar velocity assuming unit time       return( [v, ω, ψ])  return frame difference                         }                                                                             ______________________________________                                    

9.3 Frame Incrementing Subroutine

    ______________________________________                                        frameinc(F.sub.0,v.sub.0 t.sub.0)  frame incrementing subroutine              φ = |ω.sub.0 t.sub.0 |  rotation angle            k = ω.sub.0 t.sub.0 /φ  rotation axis                               S.sub.φ  = sinφ                                                       C.sub.φ  = cosφ                                                       V.sub.φ  = 1 - cosφ                                                    ##STR5##                                                                     rotation matrix                                                               .sub.1 = .sub.0  increment orientation                                        p.sub.1 = p.sub.0 + v.sub.0 t.sub.0  increment position                       ψ.sub.1 = ψ.sub.0 + ψ.sub.0 t.sub.0  increment scalar             return(F.sub.1 = {.sub.1,p.sub.1,ψ.sub.1,T.sub.0 + t.sub.0 } )            return the new                                                                frame                                                                         }                                                                             ______________________________________                                    

9.4 Calculation of Blend Time Subroutine

    ______________________________________                                        calctau(Δv)                                                             if(LINEAR)  return(Δv/2|a|.sub.max)  linear           blending                                                                      if(O3POLY)  return (3Δv/4|a|.sub.max)  third          order                                                                         polynomial blending                                                           if(CYCLOID)  return(πΔv/4|a|.sub.max)              cycloidal                                                                     blending                                                                      }                                                                             ______________________________________                                    

9.5 Calculation of Blend Profile Subroutine

    ______________________________________                                        calcfprime(s)   s is normalized time                                          if(LINEAR)  return(s)  linear blending                                        if(O3POLY)  return(-2s.sup.3 + 3s.sup.2)  third order                         polynomial blending                                                            ##STR6##                                                                     }                                                                             ______________________________________                                    

10 Conclusion

This specification has presented a new formulation of trajectorygenerated based on velocity blending. First, a new formulation fortrajectory blending was provided, allowing for the direct comparison andutilization of numerous blend functions. Then, a generalized version ofthe previously proposed orientation matrix blending formulation wasreviewed. It was shown how a first order approximation of this schemeleads directly to angular velocity blending for orientation change.Further, the residual error incurred was explained, quantized, andcompensated. Also explained were implementational considerations such asacceleration limits, velocity summation requirements, algorithmcomputation rates and complexity. Finally, the results of implementationof this scheme in both simulation and real-time experimentation weregraphically presented. Both the analysis and implementation has shownthat the speed and simplicity of the velocity-blending formulationenable its ease of use for real-time manipulator trajectory generation.

Appendix A contains a listing of a C-language computer code employed incarrying out the invention. Each of the key statements in the listing isaccompanied by an explanatory remark in italics. ##SPC1##

What is claimed is:
 1. A method of controlling a robot manipulatormoving toward a target frame F₀ with a target velocity v₀ comprising alinear target velocity ν with an angular target velocity ω₀ to smoothlyand continuously divert said robot manipulator to a subsequent frame F₁,said target frame being associated with a target transition time T₀ andsaid subsequent frame being associated with a subsequent transition timeT₁, said method comprising the steps of:determining a global transitionvelocity v₁ necessary to move said manipulator from said target frame F₀to said subsequent frame F₁ within said subsequent transition time T₁,said global transition velocity comprising a linear transition velocityν₁ and an angular transition velocity ω₁ ; defining a blend timeinterval 2τ₀ within which the global velocity of said robot manipulatoris to be changed from a global target velocity v₀ to said globaltransition velocity v₁ and dividing said blend time interval 2τ₀ intodiscrete time segments δt; during each one of said discrete timesegments δt of said blend interval 2τ₀ ; (a) computing a blended globalvelocity v of said manipulator as a blend of said global target velocityv₀ and said global transition velocity v₁, said blended global velocityv being at least approximately equal to said target global velocity v₀at the beginning of said blend time interval and at least approximatelyequal to said global transition velocity v₁ at the end of said blendtime interval, said blended global velocity v comprising a blendedangular velocity ω and a blended linear velocity ν, and (b) rotatingsaid manipulator by an incremental rotation corresponding to anintegration of said blended angular velocity ω over one discrete timesegment δt.
 2. The method of claim 1 wherein the step of defining ablend time interval comprises computing said blend time interval 2τ₀from said global target and transition velocities v₀ and v₁ and from apredetermined maximum acceleration to which motion of said manipulatoris to be limited.
 3. The method of claim 2 wherein the step of computingsaid blend time interval comprises dividing a difference between saidglobal target and transition velocities v₀ and v₁ by said predeterminedmaximum acceleration.
 4. The method of claim 2 further comprising avelocity error correction step carried out about the beginning of saidblend time interval, said velocity error correction comprising the stepsof:determining a desired blend exit frame F'₀, said step of determiningcomprising rotating said target frame F₀ through a rotationcorresponding to an integration of said angular target velocity ω overat least a portion of said blend time interval 2τ₀ ; determining anerror correction global velocity v_(e) required to move from said targetframe F₀ to said desired blend exit frame F'₀ within a correction timeinterval related to said blend time interval; and correcting said targetvelocity by adding to it said error correction velocity.
 5. The methodof claim 4 wherein said portion of said blend time interval is abouthalf said blend time interval.
 6. The method of claim 4 wherein saidcorrection time interval is a difference between said target transitiontime and half said blend time interval, T₀ -τ₀.
 7. The method of claim 4wherein the step of determining a desired blend exit frame F'₀ furthercomprises translating said target frame F₀ by a displacementcorresponding to an integration of said linear target velocity ν over atleast a portion of said blend time interval 2τ₀.
 8. The method of claim1 wherein the step of computing a blended global velocity comprisescomputing a sum of v₀ (1-ƒ)+v₁ (ƒ), wherein ƒ changes with each timeincrement δt.
 9. The method of claim 8 wherein ƒ is a function which isapproximately zero at the beginning of said blend time interval and isapproximately one at the end of said blend time interval to providelinear blending.
 10. The method of claim 8 wherein ƒ is a function whichprovides one of: (a) third order polynomial velocity blending, (b)cycloidal velocity blending.
 11. The method of claim 1 furthercomprising translating said manipulator by an incremental translationcorresponding to an integration of said blended linear velocity ν overone discrete time segment δt during each of said discrete time segments.12. The method of claim 1 wherein during a preceding time intervalimmediately prior to said blend time interval said manipulator ismaintained at an approximately constant global velocity equal to saidtarget global velocity v₀, while performing the following steps:dividingsaid preceding time interval into discrete time segments δt; during eachone of said discrete time segments of said preceding time interval,rotating said manipulator by an incremental rotation corresponding to anintegration of said target angular velocity ω₀ over one discrete timesegment δt.
 13. The method of claim 12 further comprising translatingsaid manipulator by an incremental displacement corresponding to anintegration of said target linear velocity ν₀ over one discrete timesegment δt.
 14. The method of claim 1 further comprising specifying asequence of successive target frames F_(i) associated with respectivetransition times T_(i) for i between 1 and n wherein n is an integer,and wherein after the end of each blend time interval said target frameis set to said subsequent frame and said subsequent frame is set to anext one of said successive frames.
 15. A method of controlling a robotmanipulator moving toward a target frame F₀ with a target velocity v₀comprising a linear target velocity ν with an angular target velocity ω₀to smoothly and continuously divert said robot manipulator to asubsequent frame F₁, said target frame being associated with a targettransition time T₀ and said subsequent frame being associated with asubsequent transition time T₁, said method comprising the stepsof:determining a global transition velocity v₁ necessary to move saidmanipulator from said target frame F₀ to said subsequent frame F₁ withinsaid subsequent transition time T₁, said global transition velocitycomprising a linear transition velocity ν₁ and an angular transitionvelocity ω₁ ; defining a blend time interval 2τ₀ within which the globalvelocity of said robot manipulator is to be changed from a global targetvelocity v₀ to said global transition velocity v₁ and dividing saidblend time interval 2τ₀ into discrete time segments δt; during each oneof said discrete time segments δt of said blend interval 2τ₀ ; (a)computing a blended global velocity v of said manipulator as a blend ofsaid global target velocity v₀ and said global transition velocity v₁,said blended global velocity v being at least approximately equal tosaid target global velocity v₀ at the beginning of said blend timeinterval and at least approximately equal to said global transitionvelocity v₁ at the end of said blend time interval, said blended globalvelocity v comprising a blended angular velocity ω and a blended linearvelocity ν, and (b) changing an actual global velocity of saidmanipulator in accordance with said blended angular velocity ω.
 16. Themethod of claim 15 wherein the step of defining a blend time intervalcomprises computing said blend time interval 2τ₀ from said global targetand transition velocities v₀ and v₁ and from a predetermined maximumacceleration to which motion of said manipulator is to be limited. 17.The method of claim 16 wherein the step of computing said blend timeinterval comprises dividing a difference between said global target andtransition velocities v₀ and v₁ by said predetermined maximumacceleration.
 18. The method of claim 16 further comprising a velocityerror correction step carried out about the beginning of said blend timeinterval, said velocity error correction comprising the stepsof:determining a desired blend exit frame F'₀, said step of determiningcomprising rotating said target frame F₀ through a rotationcorresponding to an integration of said angular target velocity ω overat least a portion of said blend time interval 2τ₀ ; determining anerror correction global velocity v_(e) required to move from said targetframe F₀ to said desired blend exit frame F'₀ within a correction timeinterval related to said blend time interval; and correcting said targetvelocity by adding to it said error correction velocity.
 19. The methodof claim 18 wherein said portion of said blend time interval is abouthalf said blend time interval.
 20. The method of claim 18 wherein saidcorrection time interval is a difference between said target transitiontime and half said blend time interval, T₀ -τ₀.
 21. The method of claim15 wherein the step of computing a blended global velocity comprisescomputing a sum of v₀ (1-ƒ)+v₁ (ƒ), wherein ƒ changes with each timeincrement δt.
 22. The method of claim 21 wherein ƒ is a function whichis approximately zero at the beginning of said blend time interval andis approximately one at the end of said blend time interval to providelinear blending.
 23. The method of claim 21 wherein ƒ is a functionwhich provides one of: (a) third order polynomial velocity blending, (b)cycloidal velocity blending.